#include "KMotionDef.h"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
#define CNTS_PER_INCH_X
#define CNTS_PER_INCH_Y
#define CNTS_PER_INCH_Z
#define X 0
#define Y 1
#define Z 2
#define MaxY 180000
#define MinY -1000
#define MaxZ 1000
#define MinZ -80000
#define MaxX 295000
#define MinX -1000

#define ESTOP_BIT 139
#define ZLIM_BIT 139
#define XLIM_BIT 138
#define YLIM_BIT 137


double DROx, DROy, DROz, DROa, DROb, DROc;

void SetSoftLim(int);
void home(void);
void center(void);
void estop(void);
void spindleWDog(void);
void SetZ(void);
void AxisEnable(void);
int enable=1;
int SLST=1;

main()
{
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);

    DefineCoordSystem(X,Y,Z,-1);
           
   
    FPGA(STEP_PULSE_LENGTH_ADD)=30 + 0x40 ;
     
    ch0->InputMode=ENCODER_MODE;
    ch0->OutputMode=STEP_DIR_MODE;
    ch0->Vel=87500;
    ch0->Accel=300000;
    ch0->Jerk= 4000000;
    ch0->P=0;
    ch0->I=0;
    ch0->D=0;
    ch0->FFAccel=0.0001;
    ch0->FFVel=0.003;
    ch0->MaxI=200;
    ch0->MaxErr=12;
    ch0->MaxOutput=2000;
    ch0->DeadBandGain=0;
    ch0->DeadBandRange=20;
    ch0->InputChan0=4;
    ch0->InputChan1=0;
    ch0->OutputChan0=20;
    ch0->OutputChan1=0;
    ch0->MasterAxis=-1;
    ch0->LimitSwitchOptions=0x11f;
    ch0->LimitSwitchNegBit=138;
    ch0->LimitSwitchPosBit=139;
    ch0->SoftLimitPos=MaxX;
    ch0->SoftLimitNeg=MinX;
    ch0->InputGain0=-1;
    ch0->InputGain1=1;
    ch0->InputOffset0=0;
    ch0->InputOffset1=0;
    ch0->OutputGain=-1;
    ch0->OutputOffset=0;
    ch0->SlaveGain=1;
    ch0->BacklashMode=BACKLASH_OFF;
    ch0->BacklashAmount=0;
    ch0->BacklashRate=0;
    ch0->invDistPerCycle=0;
    ch0->Lead=0;
    ch0->MaxFollowingError=12;
    ch0->StepperAmplitude=20;

    ch0->iir[0].B0=1;
    ch0->iir[0].B1=0;
    ch0->iir[0].B2=0;
    ch0->iir[0].A1=0;
    ch0->iir[0].A2=0;

    ch0->iir[1].B0=1;
    ch0->iir[1].B1=0;
    ch0->iir[1].B2=0;
    ch0->iir[1].A1=0;
    ch0->iir[1].A2=0;

    ch0->iir[2].B0=0.000768788;
    ch0->iir[2].B1=0.00153758;
    ch0->iir[2].B2=0.000768788;
    ch0->iir[2].A1=1.92076;
    ch0->iir[2].A2=-0.923833;


     
    ch1->InputMode=ENCODER_MODE;
    //ch1->InputMode=NO_INPUT_MODE;
    ch1->OutputMode=STEP_DIR_MODE;
    ch1->Vel=87500;
    ch1->Accel=300000;
    ch1->Jerk= 4000000;
    ch1->P=0;
    ch1->I=0;
    ch1->D=0;
    ch1->FFAccel=0.0001;
    ch1->FFVel=0.003;
    ch1->MaxI=200;
    ch1->MaxErr=12;
    ch1->MaxOutput=2000;
    ch1->DeadBandGain=0;
    ch1->DeadBandRange=20;
    ch1->InputChan0=1;
    ch1->InputChan1=0;
    ch1->OutputChan0=21;
    ch1->OutputChan1=0;
    ch1->MasterAxis=-1;
    ch1->LimitSwitchOptions=0x11f;
    ch1->LimitSwitchNegBit=136;
    ch1->LimitSwitchPosBit=137;
    ch1->SoftLimitPos=MaxY;
    ch1->SoftLimitNeg=MinY;
    ch1->InputGain0=-.5;
    ch1->InputGain1=1;
    ch1->InputOffset0=0;
    ch1->InputOffset1=0;
    ch1->OutputGain=-1;
    ch1->OutputOffset=0;
    ch1->SlaveGain=1;
    ch1->BacklashMode=BACKLASH_OFF;
    ch1->BacklashAmount=0;
    ch1->BacklashRate=0;
    ch1->invDistPerCycle=0;
    ch1->Lead=0;
    ch1->MaxFollowingError=24;
    ch1->StepperAmplitude=20;



    ch1->iir[0].B0=1;
    ch1->iir[0].B1=0;
    ch1->iir[0].B2=0;
    ch1->iir[0].A1=0;
    ch1->iir[0].A2=0;

    ch1->iir[1].B0=1;
    ch1->iir[1].B1=0;
    ch1->iir[1].B2=0;
    ch1->iir[1].A1=0;
    ch1->iir[1].A2=0;

    ch1->iir[2].B0=0.000769;
    ch1->iir[2].B1=0.001538;
    ch1->iir[2].B2=0.000769;
    ch1->iir[2].A1=1.92081;
    ch1->iir[2].A2=-0.923885;


   
    ch2->InputMode=ENCODER_MODE;
    ch2->OutputMode=STEP_DIR_MODE;
    ch2->Vel=50000;
    ch2->Accel=200000;
    ch2->Jerk=3000000;
    ch2->P=0;
    ch2->I=0;
    ch2->D=0;
    ch2->FFAccel=0.001;
    ch2->FFVel=0.003;
    ch2->MaxI=200;
    ch2->MaxErr=24;
    ch2->MaxOutput=2000;
    ch2->DeadBandGain=0;
    ch2->DeadBandRange=20;
    ch2->InputChan0=2;
    ch2->InputChan1=0;
    ch2->OutputChan0=19;
    ch2->OutputChan1=0;
    ch2->MasterAxis=-1;
    ch2->LimitSwitchOptions=0x11f;
    ch2->LimitSwitchNegBit=139;
    ch2->LimitSwitchPosBit=0;
    ch2->SoftLimitPos=MaxZ;
    ch2->SoftLimitNeg=MinZ;
    ch2->InputGain0=.5;
    ch2->InputGain1=1;
    ch2->InputOffset0=0;
    ch2->InputOffset1=0;
    ch2->OutputGain=1;
    ch2->OutputOffset=0;
    ch2->SlaveGain=1;
    ch2->BacklashMode=BACKLASH_OFF;
    ch2->BacklashAmount=0;
    ch2->BacklashRate=0;
    ch2->invDistPerCycle=0;
    ch2->Lead=0;
    ch2->MaxFollowingError=12;
    ch2->StepperAmplitude=20;

    ch2->iir[0].B0=1;
    ch2->iir[0].B1=0;
    ch2->iir[0].B2=0;
    ch2->iir[0].A1=0;
    ch2->iir[0].A2=0;

    ch2->iir[1].B0=1;
    ch2->iir[1].B1=0;
    ch2->iir[1].B2=0;
    ch2->iir[1].A1=0;
    ch2->iir[1].A2=0;

    ch2->iir[2].B0=0.056495;
    ch2->iir[2].B1=0.112989;
    ch2->iir[2].B2=0.056495;
    ch2->iir[2].A1=1.22565;
    ch2->iir[2].A2=-0.451634;
   
/*
        if(ESTOP_BIT && ZLIM_BIT && XLIM_BIT && YLIM_BIT )
        {
            EnableAxisDest(Y,0);  //y
            EnableAxisDest(X,0);   //z
            EnableAxisDest(Z,0);   //x 
        //    SetBit(154);
          //  SetBit(152);
            //  SetBit(153);
        }
     
      */

   
     
    while(1){







        /*if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
    */
    //home();
    //center();
    estop();
    if(CheckDoneXYZABC()==0) SetBit(155);
        else ClearBit(155);
    //spindleWDog();
    WaitNextTimeSlice();
    //SetZ();

    }
     
     
        }
     
void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
           
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
           
        }
        else if(ReadBit(55) && !enable){
        enable=1;
        EnableAxisDest(Z,ch2->Dest);  //Z
           EnableAxisDest(Y,ch1->Dest);   //Y
        EnableAxisDest(X,ch0->Dest);   //X
            SetBit(152);
            SetBit(154);
            SetBit(153);
            SetBit(155);
            ClearBit(55);
        }           
        else if(ReadBit(55) && enable){
        enable=0;
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
            ClearBit(55);
        }               
}

void SetSoftLim(void){

    if(SLST) {
       
        ch0->SoftLimitPos=MaxX;      
        ch0->SoftLimitNeg=MinX;        
        ch1->SoftLimitPos=MaxY;        
        ch1->SoftLimitNeg=MinY;      
        ch2->SoftLimitPos=MaxZ;      
        ch2->SoftLimitNeg=MinZ;       
    }
    else{
   
        ch0->SoftLimitPos= 80000;      
        ch0->SoftLimitNeg=-80000;        
        ch1->SoftLimitPos=80000;        
        ch1->SoftLimitNeg=-80000;      
        ch2->SoftLimitPos=80000;      
        ch2->SoftLimitNeg=-80000;
   
    }
    return;
}
void SetZ(void){

    if(ReadBit(53)) {
 
        ch2->SoftLimitNeg=ch2->Dest;     
            ClearBit(53);
           
    }
    if(ReadBit(54)) {
       
        ch2->SoftLimitNeg=MinZ; 
        ClearBit(54);       
    }
    return;
}
void home (void){
    if(ReadBit(49)){  //home sequence X,Y
        ClearStopImmediately();
        EnableAxisDest(X,0);
        EnableAxisDest(Y,0);   
        ClearBit(49);   
    }
    if(ReadBit(52)){  //home sequence Z   
        ClearStopImmediately();
        EnableAxisDest(Z,0);
        ClearBit(52);   
    }
    return;
}
void center(void){

    if(ReadBit(50)){  //center x
        GetDROs(&DROx, &DROy, &DROz, &DROa, &DROb, &DROc);
        DoPCFloat(PC_COMM_SET_X, DROx/2);
        ClearBit(50);
    }
    if(ReadBit(51)){  //center y
        GetDROs(&DROx, &DROy, &DROz, &DROa, &DROb, &DROc);
        DoPCFloat(PC_COMM_SET_Y, DROy/2);
        ClearBit(51);
    }
    return;
   
}


    /*
    ClearBit(48); // soft limits
    ClearBit(49); // home x and y
    ClearBit(50); // center x
    ClearBit(51); // center y
    ClearBit(52); // home z
   
    persist.UserData[10] = 0; //set current tool to zero force tool change on first M6
   
    // EnableAxisDest(Z,ch2->Dest);  //Z
       // EnableAxisDest(Y,ch1->Dest);   //Y
    // EnableAxisDest(X,ch0->Dest);   //X
   
    int lasty = ch1->Dest;   
    int lastx = ch0->Dest;   
    int lastz = ch2->Dest;

   
    while(1){







        if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
   
    home();
    center();
    //estop();
    //spindleWDog();
    WaitNextTimeSlice();
    SetZ();

    }


}



void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
            SetBit(144);
            SetBit(145);
           
        }
        else {
                ClearBit(144);
                ClearBit(145);
           
        }
}
void spindleWDog(void){
    if(ADC(0) <= 1500){ //overload condition met
        StopCoordinatedMotion();
       
        SetBit(145);
        SetBit(144);
     
    }
}



/*
void MoveX(double v); // define sub functions
void MoveY(double v);
void MoveZ(double v);
void MoveX(double v)
{
Move(X,v * CNTS_PER_INCH_X);
while (!CheckDone(X));
}

void MoveY(double v)
{
Move(Y,v * CNTS_PER_INCH_Y);
while (!CheckDone(Y));
}

void MoveZ(double v)
{
Move(Z,v * CNTS_PER_INCH_Z);
while (!CheckDone(Z));
}
*/

    // ch2->SoftLimitPos= 80000;       //y
    // ch2->SoftLimitNeg=-80000;         //y
   
    // ch1->SoftLimitPos=80000;         //z
    // ch1->SoftLimitNeg=-80000;       //z
   
    // ch0->SoftLimitPos=80000;       //x
    // ch0->SoftLimitNeg=-80000;        //x
   


   

  // EnableAxisDest(0,0);
    // EnableAxisDest(1,0);
    // EnableAxisDest(2,0);

 

I have one axis which only moves in one direction sometimes.  

E.G. I can move south (positive y) but when I try to move north the soft limit disables the axis where it sits. This happens while the axis position reports as being within the valid soft limits range.  I.E. Max is 180k, min is -1k I'm at 60k I would expect to be able to move in either direction.

Does anyone see any problems with the code below?

The problem is with axis 1. It doesn't happen every time. 

Second question, I was expecting kflop to stop movement when there is an issue with following error. That doesn't happen.

Do I need to be in closed loop step direction for that?

  If I put it in closed loop step direction and zero out pid coefficients will that do it without actually making changes to the axis positions?   

The drives have there own PID control, and I don't really trust their encoder outputs.

Thanks,

Colin

 

#include "KMotionDef.h"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
#define CNTS_PER_INCH_X
#define CNTS_PER_INCH_Y
#define CNTS_PER_INCH_Z
#define X 0
#define Y 1
#define Z 2
#define MaxY 180000
#define MinY -1000
#define MaxZ 1000
#define MinZ -80000
#define MaxX 295000
#define MinX -1000

#define ESTOP_BIT 139
#define ZLIM_BIT 139
#define XLIM_BIT 138
#define YLIM_BIT 137


double DROx, DROy, DROz, DROa, DROb, DROc;

void SetSoftLim(int);
void home(void);
void center(void);
void estop(void);
void spindleWDog(void);
void SetZ(void);
void AxisEnable(void);
int enable=1;
int SLST=1;

main()
{
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);

    DefineCoordSystem(X,Y,Z,-1);
           
   
    FPGA(STEP_PULSE_LENGTH_ADD)=30 + 0x40 ;
     
    ch0->InputMode=ENCODER_MODE;
    ch0->OutputMode=STEP_DIR_MODE;
    ch0->Vel=87500;
    ch0->Accel=300000;
    ch0->Jerk= 4000000;
    ch0->P=0;
    ch0->I=0;
    ch0->D=0;
    ch0->FFAccel=0.0001;
    ch0->FFVel=0.003;
    ch0->MaxI=200;
    ch0->MaxErr=12;
    ch0->MaxOutput=2000;
    ch0->DeadBandGain=0;
    ch0->DeadBandRange=20;
    ch0->InputChan0=4;
    ch0->InputChan1=0;
    ch0->OutputChan0=20;
    ch0->OutputChan1=0;
    ch0->MasterAxis=-1;
    ch0->LimitSwitchOptions=0x11f;
    ch0->LimitSwitchNegBit=138;
    ch0->LimitSwitchPosBit=139;
    ch0->SoftLimitPos=MaxX;
    ch0->SoftLimitNeg=MinX;
    ch0->InputGain0=-1;
    ch0->InputGain1=1;
    ch0->InputOffset0=0;
    ch0->InputOffset1=0;
    ch0->OutputGain=-1;
    ch0->OutputOffset=0;
    ch0->SlaveGain=1;
    ch0->BacklashMode=BACKLASH_OFF;
    ch0->BacklashAmount=0;
    ch0->BacklashRate=0;
    ch0->invDistPerCycle=0;
    ch0->Lead=0;
    ch0->MaxFollowingError=12;
    ch0->StepperAmplitude=20;

    ch0->iir[0].B0=1;
    ch0->iir[0].B1=0;
    ch0->iir[0].B2=0;
    ch0->iir[0].A1=0;
    ch0->iir[0].A2=0;

    ch0->iir[1].B0=1;
    ch0->iir[1].B1=0;
    ch0->iir[1].B2=0;
    ch0->iir[1].A1=0;
    ch0->iir[1].A2=0;

    ch0->iir[2].B0=0.000768788;
    ch0->iir[2].B1=0.00153758;
    ch0->iir[2].B2=0.000768788;
    ch0->iir[2].A1=1.92076;
    ch0->iir[2].A2=-0.923833;


     
    ch1->InputMode=ENCODER_MODE;
    //ch1->InputMode=NO_INPUT_MODE;
    ch1->OutputMode=STEP_DIR_MODE;
    ch1->Vel=87500;
    ch1->Accel=300000;
    ch1->Jerk= 4000000;
    ch1->P=0;
    ch1->I=0;
    ch1->D=0;
    ch1->FFAccel=0.0001;
    ch1->FFVel=0.003;
    ch1->MaxI=200;
    ch1->MaxErr=12;
    ch1->MaxOutput=2000;
    ch1->DeadBandGain=0;
    ch1->DeadBandRange=20;
    ch1->InputChan0=1;
    ch1->InputChan1=0;
    ch1->OutputChan0=21;
    ch1->OutputChan1=0;
    ch1->MasterAxis=-1;
    ch1->LimitSwitchOptions=0x11f;
    ch1->LimitSwitchNegBit=136;
    ch1->LimitSwitchPosBit=137;
    ch1->SoftLimitPos=MaxY;
    ch1->SoftLimitNeg=MinY;
    ch1->InputGain0=-.5;
    ch1->InputGain1=1;
    ch1->InputOffset0=0;
    ch1->InputOffset1=0;
    ch1->OutputGain=-1;
    ch1->OutputOffset=0;
    ch1->SlaveGain=1;
    ch1->BacklashMode=BACKLASH_OFF;
    ch1->BacklashAmount=0;
    ch1->BacklashRate=0;
    ch1->invDistPerCycle=0;
    ch1->Lead=0;
    ch1->MaxFollowingError=24;
    ch1->StepperAmplitude=20;



    ch1->iir[0].B0=1;
    ch1->iir[0].B1=0;
    ch1->iir[0].B2=0;
    ch1->iir[0].A1=0;
    ch1->iir[0].A2=0;

    ch1->iir[1].B0=1;
    ch1->iir[1].B1=0;
    ch1->iir[1].B2=0;
    ch1->iir[1].A1=0;
    ch1->iir[1].A2=0;

    ch1->iir[2].B0=0.000769;
    ch1->iir[2].B1=0.001538;
    ch1->iir[2].B2=0.000769;
    ch1->iir[2].A1=1.92081;
    ch1->iir[2].A2=-0.923885;


   
    ch2->InputMode=ENCODER_MODE;
    ch2->OutputMode=STEP_DIR_MODE;
    ch2->Vel=50000;
    ch2->Accel=200000;
    ch2->Jerk=3000000;
    ch2->P=0;
    ch2->I=0;
    ch2->D=0;
    ch2->FFAccel=0.001;
    ch2->FFVel=0.003;
    ch2->MaxI=200;
    ch2->MaxErr=24;
    ch2->MaxOutput=2000;
    ch2->DeadBandGain=0;
    ch2->DeadBandRange=20;
    ch2->InputChan0=2;
    ch2->InputChan1=0;
    ch2->OutputChan0=19;
    ch2->OutputChan1=0;
    ch2->MasterAxis=-1;
    ch2->LimitSwitchOptions=0x11f;
    ch2->LimitSwitchNegBit=139;
    ch2->LimitSwitchPosBit=0;
    ch2->SoftLimitPos=MaxZ;
    ch2->SoftLimitNeg=MinZ;
    ch2->InputGain0=.5;
    ch2->InputGain1=1;
    ch2->InputOffset0=0;
    ch2->InputOffset1=0;
    ch2->OutputGain=1;
    ch2->OutputOffset=0;
    ch2->SlaveGain=1;
    ch2->BacklashMode=BACKLASH_OFF;
    ch2->BacklashAmount=0;
    ch2->BacklashRate=0;
    ch2->invDistPerCycle=0;
    ch2->Lead=0;
    ch2->MaxFollowingError=12;
    ch2->StepperAmplitude=20;

    ch2->iir[0].B0=1;
    ch2->iir[0].B1=0;
    ch2->iir[0].B2=0;
    ch2->iir[0].A1=0;
    ch2->iir[0].A2=0;

    ch2->iir[1].B0=1;
    ch2->iir[1].B1=0;
    ch2->iir[1].B2=0;
    ch2->iir[1].A1=0;
    ch2->iir[1].A2=0;

    ch2->iir[2].B0=0.056495;
    ch2->iir[2].B1=0.112989;
    ch2->iir[2].B2=0.056495;
    ch2->iir[2].A1=1.22565;
    ch2->iir[2].A2=-0.451634;
   
/*
        if(ESTOP_BIT && ZLIM_BIT && XLIM_BIT && YLIM_BIT )
        {
            EnableAxisDest(Y,0);  //y
            EnableAxisDest(X,0);   //z
            EnableAxisDest(Z,0);   //x 
        //    SetBit(154);
          //  SetBit(152);
            //  SetBit(153);
        }
     
      */

   
     
    while(1){







        /*if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
    */
    //home();
    //center();
    estop();
    if(CheckDoneXYZABC()==0) SetBit(155);
        else ClearBit(155);
    //spindleWDog();
    WaitNextTimeSlice();
    //SetZ();

    }
     
     
        }
     
void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
           
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
           
        }
        else if(ReadBit(55) && !enable){
        enable=1;
        EnableAxisDest(Z,ch2->Dest);  //Z
           EnableAxisDest(Y,ch1->Dest);   //Y
        EnableAxisDest(X,ch0->Dest);   //X
            SetBit(152);
            SetBit(154);
            SetBit(153);
            SetBit(155);
            ClearBit(55);
        }           
        else if(ReadBit(55) && enable){
        enable=0;
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
            ClearBit(55);
        }               
}

void SetSoftLim(void){

    if(SLST) {
       
        ch0->SoftLimitPos=MaxX;      
        ch0->SoftLimitNeg=MinX;        
        ch1->SoftLimitPos=MaxY;        
        ch1->SoftLimitNeg=MinY;      
        ch2->SoftLimitPos=MaxZ;      
        ch2->SoftLimitNeg=MinZ;       
    }
    else{
   
        ch0->SoftLimitPos= 80000;      
        ch0->SoftLimitNeg=-80000;        
        ch1->SoftLimitPos=80000;        
        ch1->SoftLimitNeg=-80000;      
        ch2->SoftLimitPos=80000;      
        ch2->SoftLimitNeg=-80000;
   
    }
    return;
}
void SetZ(void){

    if(ReadBit(53)) {
 
        ch2->SoftLimitNeg=ch2->Dest;     
            ClearBit(53);
           
    }
    if(ReadBit(54)) {
       
        ch2->SoftLimitNeg=MinZ; 
        ClearBit(54);       
    }
    return;
}
void home (void){
    if(ReadBit(49)){  //home sequence X,Y
        ClearStopImmediately();
        EnableAxisDest(X,0);
        EnableAxisDest(Y,0);   
        ClearBit(49);   
    }
    if(ReadBit(52)){  //home sequence Z   
        ClearStopImmediately();
        EnableAxisDest(Z,0);
        ClearBit(52);   
    }
    return;
}
void center(void){

    if(ReadBit(50)){  //center x
        GetDROs(&DROx, &DROy, &DROz, &DROa, &DROb, &DROc);
        DoPCFloat(PC_COMM_SET_X, DROx/2);
        ClearBit(50);
    }
    if(ReadBit(51)){  //center y
        GetDROs(&DROx, &DROy, &DROz, &DROa, &DROb, &DROc);
        DoPCFloat(PC_COMM_SET_Y, DROy/2);
        ClearBit(51);
    }
    return;
   
}


    /*
    ClearBit(48); // soft limits
    ClearBit(49); // home x and y
    ClearBit(50); // center x
    ClearBit(51); // center y
    ClearBit(52); // home z
   
    persist.UserData[10] = 0; //set current tool to zero force tool change on first M6
   
    // EnableAxisDest(Z,ch2->Dest);  //Z
       // EnableAxisDest(Y,ch1->Dest);   //Y
    // EnableAxisDest(X,ch0->Dest);   //X
   
    int lasty = ch1->Dest;   
    int lastx = ch0->Dest;   
    int lastz = ch2->Dest;

   
    while(1){







        if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
   
    home();
    center();
    //estop();
    //spindleWDog();
    WaitNextTimeSlice();
    SetZ();

    }


}



void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
            SetBit(144);
            SetBit(145);
           
        }
        else {
                ClearBit(144);
                ClearBit(145);
           
        }
}
void spindleWDog(void){
    if(ADC(0) <= 1500){ //overload condition met
        StopCoordinatedMotion();
       
        SetBit(145);
        SetBit(144);
     
    }
}



/*
void MoveX(double v); // define sub functions
void MoveY(double v);
void MoveZ(double v);
void MoveX(double v)
{
Move(X,v * CNTS_PER_INCH_X);
while (!CheckDone(X));
}

void MoveY(double v)
{
Move(Y,v * CNTS_PER_INCH_Y);
while (!CheckDone(Y));
}

void MoveZ(double v)
{
Move(Z,v * CNTS_PER_INCH_Z);
while (!CheckDone(Z));
}
*/

    // ch2->SoftLimitPos= 80000;       //y
    // ch2->SoftLimitNeg=-80000;         //y
   
    // ch1->SoftLimitPos=80000;         //z
    // ch1->SoftLimitNeg=-80000;       //z
   
    // ch0->SoftLimitPos=80000;       //x
    // ch0->SoftLimitNeg=-80000;        //x
   


   

  // EnableAxisDest(0,0);
    // EnableAxisDest(1,0);
    // EnableAxisDest(2,0);

 

Hi Tom,

Sorry for the dirty code.

You were 100% correct it wasn't a soft limit issue. I had the wrong limit switch input bit. The reason I don't want KFLOP doing closed loop is that I don't trust the feedback from the drives.  

Thanks,

Colin

On Wed, Jan 6, 2016 at 10:43 AM, Tom Kerekes tk@... [DynoMotion] <DynoMotion@yahoogroups.com> wrote:

 

Hi Colin,

Soft Limits should not disable the axis but rather issue a feed hold.  Why is it you think that you have a problem with Soft Limits?

Please clean up your C Program and remove unused, duplicated, commented out code, and indent the code properly to make it more readable.

Regarding Following error.  Yes you are correct.  Following error is only checked in closed loop modes.  Changed to Closed Loop with PID+FF Gains all Zero if you don't wish to make any corrections.

Regards
TK

On 1/6/2016 7:53 AM, Colin Fera colin.fera@... [DynoMotion] wrote:

 

I have one axis which only moves in one direction sometimes.  

E.G. I can move south (positive y) but when I try to move north the soft limit disables the axis where it sits. This happens while the axis position reports as being within the valid soft limits range.  I.E. Max is 180k, min is -1k I'm at 60k I would expect to be able to move in either direction.

Does anyone see any problems with the code below?

The problem is with axis 1. It doesn't happen every time. 

Second question, I was expecting kflop to stop movement when there is an issue with following error. That doesn't happen.

Do I need to be in closed loop step direction for that?

  If I put it in closed loop step direction and zero out pid coefficients will that do it without actually making changes to the axis positions?   

The drives have there own PID control, and I don't really trust their encoder outputs.

Thanks,

Colin

 

#include "KMotionDef.h"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
#define CNTS_PER_INCH_X
#define CNTS_PER_INCH_Y
#define CNTS_PER_INCH_Z
#define X 0
#define Y 1
#define Z 2
#define MaxY 180000
#define MinY -1000
#define MaxZ 1000
#define MinZ -80000
#define MaxX 295000
#define MinX -1000

#define ESTOP_BIT 139
#define ZLIM_BIT 139
#define XLIM_BIT 138
#define YLIM_BIT 137


double DROx, DROy, DROz, DROa, DROb, DROc;

void SetSoftLim(int);
void home(void);
void center(void);
void estop(void);
void spindleWDog(void);
void SetZ(void);
void AxisEnable(void);
int enable=1;
int SLST=1;

main()
{
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);

    DefineCoordSystem(X,Y,Z,-1);
           
   
    FPGA(STEP_PULSE_LENGTH_ADD)=30 + 0x40 ;
     
    ch0->InputMode=ENCODER_MODE;
    ch0->OutputMode=STEP_DIR_MODE;
    ch0->Vel=87500;
    ch0->Accel=300000;
    ch0->Jerk= 4000000;
    ch0->P=0;
    ch0->I=0;
    ch0->D=0;
    ch0->FFAccel=0.0001;
    ch0->FFVel=0.003;
    ch0->MaxI=200;
    ch0->MaxErr=12;
    ch0->MaxOutput=2000;
    ch0->DeadBandGain=0;
    ch0->DeadBandRange=20;
    ch0->InputChan0=4;
    ch0->InputChan1=0;
    ch0->OutputChan0=20;
    ch0->OutputChan1=0;
    ch0->MasterAxis=-1;
    ch0->LimitSwitchOptions=0x11f;
    ch0->LimitSwitchNegBit=138;
    ch0->LimitSwitchPosBit=139;
    ch0->SoftLimitPos=MaxX;
    ch0->SoftLimitNeg=MinX;
    ch0->InputGain0=-1;
    ch0->InputGain1=1;
    ch0->InputOffset0=0;
    ch0->InputOffset1=0;
    ch0->OutputGain=-1;
    ch0->OutputOffset=0;
    ch0->SlaveGain=1;
    ch0->BacklashMode=BACKLASH_OFF;
    ch0->BacklashAmount=0;
    ch0->BacklashRate=0;
    ch0->invDistPerCycle=0;
    ch0->Lead=0;
    ch0->MaxFollowingError=12;
    ch0->StepperAmplitude=20;

    ch0->iir[0].B0=1;
    ch0->iir[0].B1=0;
    ch0->iir[0].B2=0;
    ch0->iir[0].A1=0;
    ch0->iir[0].A2=0;

    ch0->iir[1].B0=1;
    ch0->iir[1].B1=0;
    ch0->iir[1].B2=0;
    ch0->iir[1].A1=0;
    ch0->iir[1].A2=0;

    ch0->iir[2].B0=0.000768788;
    ch0->iir[2].B1=0.00153758;
    ch0->iir[2].B2=0.000768788;
    ch0->iir[2].A1=1.92076;
    ch0->iir[2].A2=-0.923833;


     
    ch1->InputMode=ENCODER_MODE;
    //ch1->InputMode=NO_INPUT_MODE;
    ch1->OutputMode=STEP_DIR_MODE;
    ch1->Vel=87500;
    ch1->Accel=300000;
    ch1->Jerk= 4000000;
    ch1->P=0;
    ch1->I=0;
    ch1->D=0;
    ch1->FFAccel=0.0001;
    ch1->FFVel=0.003;
    ch1->MaxI=200;
    ch1->MaxErr=12;
    ch1->MaxOutput=2000;
    ch1->DeadBandGain=0;
    ch1->DeadBandRange=20;
    ch1->InputChan0=1;
    ch1->InputChan1=0;
    ch1->OutputChan0=21;
    ch1->OutputChan1=0;
    ch1->MasterAxis=-1;
    ch1->LimitSwitchOptions=0x11f;
    ch1->LimitSwitchNegBit=136;
    ch1->LimitSwitchPosBit=137;
    ch1->SoftLimitPos=MaxY;
    ch1->SoftLimitNeg=MinY;
    ch1->InputGain0=-.5;
    ch1->InputGain1=1;
    ch1->InputOffset0=0;
    ch1->InputOffset1=0;
    ch1->OutputGain=-1;
    ch1->OutputOffset=0;
    ch1->SlaveGain=1;
    ch1->BacklashMode=BACKLASH_OFF;
    ch1->BacklashAmount=0;
    ch1->BacklashRate=0;
    ch1->invDistPerCycle=0;
    ch1->Lead=0;
    ch1->MaxFollowingError=24;
    ch1->StepperAmplitude=20;



    ch1->iir[0].B0=1;
    ch1->iir[0].B1=0;
    ch1->iir[0].B2=0;
    ch1->iir[0].A1=0;
    ch1->iir[0].A2=0;

    ch1->iir[1].B0=1;
    ch1->iir[1].B1=0;
    ch1->iir[1].B2=0;
    ch1->iir[1].A1=0;
    ch1->iir[1].A2=0;

    ch1->iir[2].B0=0.000769;
    ch1->iir[2].B1=0.001538;
    ch1->iir[2].B2=0.000769;
    ch1->iir[2].A1=1.92081;
    ch1->iir[2].A2=-0.923885;


   
    ch2->InputMode=ENCODER_MODE;
    ch2->OutputMode=STEP_DIR_MODE;
    ch2->Vel=50000;
    ch2->Accel=200000;
    ch2->Jerk=3000000;
    ch2->P=0;
    ch2->I=0;
    ch2->D=0;
    ch2->FFAccel=0.001;
    ch2->FFVel=0.003;
    ch2->MaxI=200;
    ch2->MaxErr=24;
    ch2->MaxOutput=2000;
    ch2->DeadBandGain=0;
    ch2->DeadBandRange=20;
    ch2->InputChan0=2;
    ch2->InputChan1=0;
    ch2->OutputChan0=19;
    ch2->OutputChan1=0;
    ch2->MasterAxis=-1;
    ch2->LimitSwitchOptions=0x11f;
    ch2->LimitSwitchNegBit=139;
    ch2->LimitSwitchPosBit=0;
    ch2->SoftLimitPos=MaxZ;
    ch2->SoftLimitNeg=MinZ;
    ch2->InputGain0=.5;
    ch2->InputGain1=1;
    ch2->InputOffset0=0;
    ch2->InputOffset1=0;
    ch2->OutputGain=1;
    ch2->OutputOffset=0;
    ch2->SlaveGain=1;
    ch2->BacklashMode=BACKLASH_OFF;
    ch2->BacklashAmount=0;
    ch2->BacklashRate=0;
    ch2->invDistPerCycle=0;
    ch2->Lead=0;
    ch2->MaxFollowingError=12;
    ch2->StepperAmplitude=20;

    ch2->iir[0].B0=1;
    ch2->iir[0].B1=0;
    ch2->iir[0].B2=0;
    ch2->iir[0].A1=0;
    ch2->iir[0].A2=0;

    ch2->iir[1].B0=1;
    ch2->iir[1].B1=0;
    ch2->iir[1].B2=0;
    ch2->iir[1].A1=0;
    ch2->iir[1].A2=0;

    ch2->iir[2].B0=0.056495;
    ch2->iir[2].B1=0.112989;
    ch2->iir[2].B2=0.056495;
    ch2->iir[2].A1=1.22565;
    ch2->iir[2].A2=-0.451634;
   
/*
        if(ESTOP_BIT && ZLIM_BIT && XLIM_BIT && YLIM_BIT )
        {
            EnableAxisDest(Y,0);  //y
            EnableAxisDest(X,0);   //z
            EnableAxisDest(Z,0);   //x 
        //    SetBit(154);
          //  SetBit(152);
            //  SetBit(153);
        }
     
      */

   
     
    while(1){







        /*if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
    */
    //home();
    //center();
    estop();
    if(CheckDoneXYZABC()==0) SetBit(155);
        else ClearBit(155);
    //spindleWDog();
    WaitNextTimeSlice();
    //SetZ();

    }
     
     
        }
     
void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
           
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
           
        }
        else if(ReadBit(55) && !enable){
        enable=1;
        EnableAxisDest(Z,ch2->Dest);  //Z
           EnableAxisDest(Y,ch1->Dest);   //Y
        EnableAxisDest(X,ch0->Dest);   //X
            SetBit(152);
            SetBit(154);
            SetBit(153);
            SetBit(155);
            ClearBit(55);
        }           
        else if(ReadBit(55) && enable){
        enable=0;
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
            ClearBit(55);
        }               
}

void SetSoftLim(void){

    if(SLST) {
       
        ch0->SoftLimitPos=MaxX;      
        ch0->SoftLimitNeg=MinX;        
        ch1->SoftLimitPos=MaxY;        
        ch1->SoftLimitNeg=MinY;      
        ch2->SoftLimitPos=MaxZ;      
        ch2->SoftLimitNeg=MinZ;       
    }
    else{
   
        ch0->SoftLimitPos= 80000;      
        ch0->SoftLimitNeg=-80000;        
        ch1->SoftLimitPos=80000;        
        ch1->SoftLimitNeg=-80000;      
        ch2->SoftLimitPos=80000;      
        ch2->SoftLimitNeg=-80000;
   
    }
    return;
}
void SetZ(void){

    if(ReadBit(53)) {
 
        ch2->SoftLimitNeg=ch2->Dest;     
            ClearBit(53);
           
    }
    if(ReadBit(54)) {
       
        ch2->SoftLimitNeg=MinZ; 
        ClearBit(54);       
    }
    return;
}
void home (void){
    if(ReadBit(49)){  //home sequence X,Y
        ClearStopImmediately();
        EnableAxisDest(X,0);
        EnableAxisDest(Y,0);   
        ClearBit(49);   
    }
    if(ReadBit(52)){  //home sequence Z   
        ClearStopImmediately();
        EnableAxisDest(Z,0);
        ClearBit(52);   
    }
    return;
}
void center(void){

    if(ReadBit(50)){  //center x
        GetDROs(&DROx, &DROy, &DROz, &DROa, &DROb, &DROc);
        DoPCFloat(PC_COMM_SET_X, DROx/2);
        ClearBit(50);
    }
    if(ReadBit(51)){  //center y
        GetDROs(&DROx, &DROy, &DROz, &DROa, &DROb, &DROc);
        DoPCFloat(PC_COMM_SET_Y, DROy/2);
        ClearBit(51);
    }
    return;
   
}


    /*
    ClearBit(48); // soft limits
    ClearBit(49); // home x and y
    ClearBit(50); // center x
    ClearBit(51); // center y
    ClearBit(52); // home z
   
    persist.UserData[10] = 0; //set current tool to zero force tool change on first M6
   
    // EnableAxisDest(Z,ch2->Dest);  //Z
       // EnableAxisDest(Y,ch1->Dest);   //Y
    // EnableAxisDest(X,ch0->Dest);   //X
   
    int lasty = ch1->Dest;   
    int lastx = ch0->Dest;   
    int lastz = ch2->Dest;

   
    while(1){







        if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
   
    home();
    center();
    //estop();
    //spindleWDog();
    WaitNextTimeSlice();
    SetZ();

    }


}



void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
            SetBit(144);
            SetBit(145);
           
        }
        else {
                ClearBit(144);
                ClearBit(145);
           
        }
}
void spindleWDog(void){
    if(ADC(0) <= 1500){ //overload condition met
        StopCoordinatedMotion();
       
        SetBit(145);
        SetBit(144);
     
    }
}



/*
void MoveX(double v); // define sub functions
void MoveY(double v);
void MoveZ(double v);
void MoveX(double v)
{
Move(X,v * CNTS_PER_INCH_X);
while (!CheckDone(X));
}

void MoveY(double v)
{
Move(Y,v * CNTS_PER_INCH_Y);
while (!CheckDone(Y));
}

void MoveZ(double v)
{
Move(Z,v * CNTS_PER_INCH_Z);
while (!CheckDone(Z));
}
*/

    // ch2->SoftLimitPos= 80000;       //y
    // ch2->SoftLimitNeg=-80000;         //y
   
    // ch1->SoftLimitPos=80000;         //z
    // ch1->SoftLimitNeg=-80000;       //z
   
    // ch0->SoftLimitPos=80000;       //x
    // ch0->SoftLimitNeg=-80000;        //x
   


   

  // EnableAxisDest(0,0);
    // EnableAxisDest(1,0);
    // EnableAxisDest(2,0);

 

Hi Tom,

I intend to use the following error to stop all movement in the event that some critical event like a crash happens. When I say I don't trust them, I mean I don't trust them for very tight control.

-Colin

On Fri, Jan 8, 2016 at 12:03 PM, Tom Kerekes tk@... [DynoMotion] <DynoMotion@yahoogroups.com> wrote:

 

Hi Colin,

If you don't trust the encoder feedback it isn't clear to me why you wish to use it determine if there is a following error?

Regards
TK

On 1/8/2016 11:38 AM, Colin Fera colin.fera@... [DynoMotion] wrote:

 

Hi Tom,

Sorry for the dirty code.

You were 100% correct it wasn't a soft limit issue. I had the wrong limit switch input bit. The reason I don't want KFLOP doing closed loop is that I don't trust the feedback from the drives.  

Thanks,

Colin

On Wed, Jan 6, 2016 at 10:43 AM, Tom Kerekes tk@... [DynoMotion] <DynoMotion@yahoogroups.com> wrote:

 

Hi Colin,

Soft Limits should not disable the axis but rather issue a feed hold.  Why is it you think that you have a problem with Soft Limits?

Please clean up your C Program and remove unused, duplicated, commented out code, and indent the code properly to make it more readable.

Regarding Following error.  Yes you are correct.  Following error is only checked in closed loop modes.  Changed to Closed Loop with PID+FF Gains all Zero if you don't wish to make any corrections.

Regards
TK

On 1/6/2016 7:53 AM, Colin Fera colin.fera@... [DynoMotion] wrote:

 

I have one axis which only moves in one direction sometimes.  

E.G. I can move south (positive y) but when I try to move north the soft limit disables the axis where it sits. This happens while the axis position reports as being within the valid soft limits range.  I.E. Max is 180k, min is -1k I'm at 60k I would expect to be able to move in either direction.

Does anyone see any problems with the code below?

The problem is with axis 1. It doesn't happen every time. 

Second question, I was expecting kflop to stop movement when there is an issue with following error. That doesn't happen.

Do I need to be in closed loop step direction for that?

  If I put it in closed loop step direction and zero out pid coefficients will that do it without actually making changes to the axis positions?   

The drives have there own PID control, and I don't really trust their encoder outputs.

Thanks,

Colin

 

#include "KMotionDef.h"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
#define CNTS_PER_INCH_X
#define CNTS_PER_INCH_Y
#define CNTS_PER_INCH_Z
#define X 0
#define Y 1
#define Z 2
#define MaxY 180000
#define MinY -1000
#define MaxZ 1000
#define MinZ -80000
#define MaxX 295000
#define MinX -1000

#define ESTOP_BIT 139
#define ZLIM_BIT 139
#define XLIM_BIT 138
#define YLIM_BIT 137


double DROx, DROy, DROz, DROa, DROb, DROc;

void SetSoftLim(int);
void home(void);
void center(void);
void estop(void);
void spindleWDog(void);
void SetZ(void);
void AxisEnable(void);
int enable=1;
int SLST=1;

main()
{
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);

    DefineCoordSystem(X,Y,Z,-1);
           
   
    FPGA(STEP_PULSE_LENGTH_ADD)=30 + 0x40 ;
     
    ch0->InputMode=ENCODER_MODE;
    ch0->OutputMode=STEP_DIR_MODE;
    ch0->Vel=87500;
    ch0->Accel=300000;
    ch0->Jerk= 4000000;
    ch0->P=0;
    ch0->I=0;
    ch0->D=0;
    ch0->FFAccel=0.0001;
    ch0->FFVel=0.003;
    ch0->MaxI=200;
    ch0->MaxErr=12;
    ch0->MaxOutput=2000;
    ch0->DeadBandGain=0;
    ch0->DeadBandRange=20;
    ch0->InputChan0=4;
    ch0->InputChan1=0;
    ch0->OutputChan0=20;
    ch0->OutputChan1=0;
    ch0->MasterAxis=-1;
    ch0->LimitSwitchOptions=0x11f;
    ch0->LimitSwitchNegBit=138;
    ch0->LimitSwitchPosBit=139;
    ch0->SoftLimitPos=MaxX;
    ch0->SoftLimitNeg=MinX;
    ch0->InputGain0=-1;
    ch0->InputGain1=1;
    ch0->InputOffset0=0;
    ch0->InputOffset1=0;
    ch0->OutputGain=-1;
    ch0->OutputOffset=0;
    ch0->SlaveGain=1;
    ch0->BacklashMode=BACKLASH_OFF;
    ch0->BacklashAmount=0;
    ch0->BacklashRate=0;
    ch0->invDistPerCycle=0;
    ch0->Lead=0;
    ch0->MaxFollowingError=12;
    ch0->StepperAmplitude=20;

    ch0->iir[0].B0=1;
    ch0->iir[0].B1=0;
    ch0->iir[0].B2=0;
    ch0->iir[0].A1=0;
    ch0->iir[0].A2=0;

    ch0->iir[1].B0=1;
    ch0->iir[1].B1=0;
    ch0->iir[1].B2=0;
    ch0->iir[1].A1=0;
    ch0->iir[1].A2=0;

    ch0->iir[2].B0=0.000768788;
    ch0->iir[2].B1=0.00153758;
    ch0->iir[2].B2=0.000768788;
    ch0->iir[2].A1=1.92076;
    ch0->iir[2].A2=-0.923833;


     
    ch1->InputMode=ENCODER_MODE;
    //ch1->InputMode=NO_INPUT_MODE;
    ch1->OutputMode=STEP_DIR_MODE;
    ch1->Vel=87500;
    ch1->Accel=300000;
    ch1->Jerk= 4000000;
    ch1->P=0;
    ch1->I=0;
    ch1->D=0;
    ch1->FFAccel=0.0001;
    ch1->FFVel=0.003;
    ch1->MaxI=200;
    ch1->MaxErr=12;
    ch1->MaxOutput=2000;
    ch1->DeadBandGain=0;
    ch1->DeadBandRange=20;
    ch1->InputChan0=1;
    ch1->InputChan1=0;
    ch1->OutputChan0=21;
    ch1->OutputChan1=0;
    ch1->MasterAxis=-1;
    ch1->LimitSwitchOptions=0x11f;
    ch1->LimitSwitchNegBit=136;
    ch1->LimitSwitchPosBit=137;
    ch1->SoftLimitPos=MaxY;
    ch1->SoftLimitNeg=MinY;
    ch1->InputGain0=-.5;
    ch1->InputGain1=1;
    ch1->InputOffset0=0;
    ch1->InputOffset1=0;
    ch1->OutputGain=-1;
    ch1->OutputOffset=0;
    ch1->SlaveGain=1;
    ch1->BacklashMode=BACKLASH_OFF;
    ch1->BacklashAmount=0;
    ch1->BacklashRate=0;
    ch1->invDistPerCycle=0;
    ch1->Lead=0;
    ch1->MaxFollowingError=24;
    ch1->StepperAmplitude=20;



    ch1->iir[0].B0=1;
    ch1->iir[0].B1=0;
    ch1->iir[0].B2=0;
    ch1->iir[0].A1=0;
    ch1->iir[0].A2=0;

    ch1->iir[1].B0=1;
    ch1->iir[1].B1=0;
    ch1->iir[1].B2=0;
    ch1->iir[1].A1=0;
    ch1->iir[1].A2=0;

    ch1->iir[2].B0=0.000769;
    ch1->iir[2].B1=0.001538;
    ch1->iir[2].B2=0.000769;
    ch1->iir[2].A1=1.92081;
    ch1->iir[2].A2=-0.923885;


   
    ch2->InputMode=ENCODER_MODE;
    ch2->OutputMode=STEP_DIR_MODE;
    ch2->Vel=50000;
    ch2->Accel=200000;
    ch2->Jerk=3000000;
    ch2->P=0;
    ch2->I=0;
    ch2->D=0;
    ch2->FFAccel=0.001;
    ch2->FFVel=0.003;
    ch2->MaxI=200;
    ch2->MaxErr=24;
    ch2->MaxOutput=2000;
    ch2->DeadBandGain=0;
    ch2->DeadBandRange=20;
    ch2->InputChan0=2;
    ch2->InputChan1=0;
    ch2->OutputChan0=19;
    ch2->OutputChan1=0;
    ch2->MasterAxis=-1;
    ch2->LimitSwitchOptions=0x11f;
    ch2->LimitSwitchNegBit=139;
    ch2->LimitSwitchPosBit=0;
    ch2->SoftLimitPos=MaxZ;
    ch2->SoftLimitNeg=MinZ;
    ch2->InputGain0=.5;
    ch2->InputGain1=1;
    ch2->InputOffset0=0;
    ch2->InputOffset1=0;
    ch2->OutputGain=1;
    ch2->OutputOffset=0;
    ch2->SlaveGain=1;
    ch2->BacklashMode=BACKLASH_OFF;
    ch2->BacklashAmount=0;
    ch2->BacklashRate=0;
    ch2->invDistPerCycle=0;
    ch2->Lead=0;
    ch2->MaxFollowingError=12;
    ch2->StepperAmplitude=20;

    ch2->iir[0].B0=1;
    ch2->iir[0].B1=0;
    ch2->iir[0].B2=0;
    ch2->iir[0].A1=0;
    ch2->iir[0].A2=0;

    ch2->iir[1].B0=1;
    ch2->iir[1].B1=0;
    ch2->iir[1].B2=0;
    ch2->iir[1].A1=0;
    ch2->iir[1].A2=0;

    ch2->iir[2].B0=0.056495;
    ch2->iir[2].B1=0.112989;
    ch2->iir[2].B2=0.056495;
    ch2->iir[2].A1=1.22565;
    ch2->iir[2].A2=-0.451634;
   
/*
        if(ESTOP_BIT && ZLIM_BIT && XLIM_BIT && YLIM_BIT )
        {
            EnableAxisDest(Y,0);  //y
            EnableAxisDest(X,0);   //z
            EnableAxisDest(Z,0);   //x 
        //    SetBit(154);
          //  SetBit(152);
            //  SetBit(153);
        }
     
      */

   
     
    while(1){







        /*if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
    */
    //home();
    //center();
    estop();
    if(CheckDoneXYZABC()==0) SetBit(155);
        else ClearBit(155);
    //spindleWDog();
    WaitNextTimeSlice();
    //SetZ();

    }
     
     
        }
     
void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
           
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
           
        }
        else if(ReadBit(55) && !enable){
        enable=1;
        EnableAxisDest(Z,ch2->Dest);  //Z
           EnableAxisDest(Y,ch1->Dest);   //Y
        EnableAxisDest(X,ch0->Dest);   //X
            SetBit(152);
            SetBit(154);
            SetBit(153);
            SetBit(155);
            ClearBit(55);
        }           
        else if(ReadBit(55) && enable){
        enable=0;
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);
            ClearBit(55);
        }               
}

void SetSoftLim(void){

    if(SLST) {
       
        ch0->SoftLimitPos=MaxX;      
       

(Message over 64 KB, truncated)

 

Hi Tom,

Is it possible to change the number of digits displayed in the DRO in kMotionCNC?

This is really for testing/experimentation.

Thanks,

-Colin

On Fri, Jan 8, 2016 at 3:21 PM, Tom Kerekes tk@... [DynoMotion] <DynoMotion@yahoogroups.com> wrote:

 

Makes sense Colin.

TK

On 1/8/2016 12:57 PM, Colin Fera colin.fera@... [DynoMotion] wrote:

 

Hi Tom,

I intend to use the following error to stop all movement in the event that some critical event like a crash happens. When I say I don't trust them, I mean I don't trust them for very tight control.

-Colin

On Fri, Jan 8, 2016 at 12:03 PM, Tom Kerekes tk@... [DynoMotion] <DynoMotion@yahoogroups.com> wrote:

 

Hi Colin,

If you don't trust the encoder feedback it isn't clear to me why you wish to use it determine if there is a following error?

Regards
TK

On 1/8/2016 11:38 AM, Colin Fera colin.fera@... [DynoMotion] wrote:

 

Hi Tom,

Sorry for the dirty code.

You were 100% correct it wasn't a soft limit issue. I had the wrong limit switch input bit. The reason I don't want KFLOP doing closed loop is that I don't trust the feedback from the drives.  

Thanks,

Colin

On Wed, Jan 6, 2016 at 10:43 AM, Tom Kerekes tk@... [DynoMotion] <DynoMotion@yahoogroups.com> wrote:

 

Hi Colin,

Soft Limits should not disable the axis but rather issue a feed hold.  Why is it you think that you have a problem with Soft Limits?

Please clean up your C Program and remove unused, duplicated, commented out code, and indent the code properly to make it more readable.

Regarding Following error.  Yes you are correct.  Following error is only checked in closed loop modes.  Changed to Closed Loop with PID+FF Gains all Zero if you don't wish to make any corrections.

Regards
TK

On 1/6/2016 7:53 AM, Colin Fera colin.fera@... [DynoMotion] wrote:

 

I have one axis which only moves in one direction sometimes.  

E.G. I can move south (positive y) but when I try to move north the soft limit disables the axis where it sits. This happens while the axis position reports as being within the valid soft limits range.  I.E. Max is 180k, min is -1k I'm at 60k I would expect to be able to move in either direction.

Does anyone see any problems with the code below?

The problem is with axis 1. It doesn't happen every time. 

Second question, I was expecting kflop to stop movement when there is an issue with following error. That doesn't happen.

Do I need to be in closed loop step direction for that?

  If I put it in closed loop step direction and zero out pid coefficients will that do it without actually making changes to the axis positions?   

The drives have there own PID control, and I don't really trust their encoder outputs.

Thanks,

Colin

 

#include "KMotionDef.h"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
#define CNTS_PER_INCH_X
#define CNTS_PER_INCH_Y
#define CNTS_PER_INCH_Z
#define X 0
#define Y 1
#define Z 2
#define MaxY 180000
#define MinY -1000
#define MaxZ 1000
#define MinZ -80000
#define MaxX 295000
#define MinX -1000

#define ESTOP_BIT 139
#define ZLIM_BIT 139
#define XLIM_BIT 138
#define YLIM_BIT 137


double DROx, DROy, DROz, DROa, DROb, DROc;

void SetSoftLim(int);
void home(void);
void center(void);
void estop(void);
void spindleWDog(void);
void SetZ(void);
void AxisEnable(void);
int enable=1;
int SLST=1;

main()
{
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if (ch0->Enable) DisableAxis(X);  // axis still enabled?  - Disable it
       
            ClearBit(152);
            ClearBit(154);
            ClearBit(153);
            ClearBit(155);

    DefineCoordSystem(X,Y,Z,-1);
           
   
    FPGA(STEP_PULSE_LENGTH_ADD)=30 + 0x40 ;
     
    ch0->InputMode=ENCODER_MODE;
    ch0->OutputMode=STEP_DIR_MODE;
    ch0->Vel=87500;
    ch0->Accel=300000;
    ch0->Jerk= 4000000;
    ch0->P=0;
    ch0->I=0;
    ch0->D=0;
    ch0->FFAccel=0.0001;
    ch0->FFVel=0.003;
    ch0->MaxI=200;
    ch0->MaxErr=12;
    ch0->MaxOutput=2000;
    ch0->DeadBandGain=0;
    ch0->DeadBandRange=20;
    ch0->InputChan0=4;
    ch0->InputChan1=0;
    ch0->OutputChan0=20;
    ch0->OutputChan1=0;
    ch0->MasterAxis=-1;
    ch0->LimitSwitchOptions=0x11f;
    ch0->LimitSwitchNegBit=138;
    ch0->LimitSwitchPosBit=139;
    ch0->SoftLimitPos=MaxX;
    ch0->SoftLimitNeg=MinX;
    ch0->InputGain0=-1;
    ch0->InputGain1=1;
    ch0->InputOffset0=0;
    ch0->InputOffset1=0;
    ch0->OutputGain=-1;
    ch0->OutputOffset=0;
    ch0->SlaveGain=1;
    ch0->BacklashMode=BACKLASH_OFF;
    ch0->BacklashAmount=0;
    ch0->BacklashRate=0;
    ch0->invDistPerCycle=0;
    ch0->Lead=0;
    ch0->MaxFollowingError=12;
    ch0->StepperAmplitude=20;

    ch0->iir[0].B0=1;
    ch0->iir[0].B1=0;
    ch0->iir[0].B2=0;
    ch0->iir[0].A1=0;
    ch0->iir[0].A2=0;

    ch0->iir[1].B0=1;
    ch0->iir[1].B1=0;
    ch0->iir[1].B2=0;
    ch0->iir[1].A1=0;
    ch0->iir[1].A2=0;

    ch0->iir[2].B0=0.000768788;
    ch0->iir[2].B1=0.00153758;
    ch0->iir[2].B2=0.000768788;
    ch0->iir[2].A1=1.92076;
    ch0->iir[2].A2=-0.923833;


     
    ch1->InputMode=ENCODER_MODE;
    //ch1->InputMode=NO_INPUT_MODE;
    ch1->OutputMode=STEP_DIR_MODE;
    ch1->Vel=87500;
    ch1->Accel=300000;
    ch1->Jerk= 4000000;
    ch1->P=0;
    ch1->I=0;
    ch1->D=0;
    ch1->FFAccel=0.0001;
    ch1->FFVel=0.003;
    ch1->MaxI=200;
    ch1->MaxErr=12;
    ch1->MaxOutput=2000;
    ch1->DeadBandGain=0;
    ch1->DeadBandRange=20;
    ch1->InputChan0=1;
    ch1->InputChan1=0;
    ch1->OutputChan0=21;
    ch1->OutputChan1=0;
    ch1->MasterAxis=-1;
    ch1->LimitSwitchOptions=0x11f;
    ch1->LimitSwitchNegBit=136;
    ch1->LimitSwitchPosBit=137;
    ch1->SoftLimitPos=MaxY;
    ch1->SoftLimitNeg=MinY;
    ch1->InputGain0=-.5;
    ch1->InputGain1=1;
    ch1->InputOffset0=0;
    ch1->InputOffset1=0;
    ch1->OutputGain=-1;
    ch1->OutputOffset=0;
    ch1->SlaveGain=1;
    ch1->BacklashMode=BACKLASH_OFF;
    ch1->BacklashAmount=0;
    ch1->BacklashRate=0;
    ch1->invDistPerCycle=0;
    ch1->Lead=0;
    ch1->MaxFollowingError=24;
    ch1->StepperAmplitude=20;



    ch1->iir[0].B0=1;
    ch1->iir[0].B1=0;
    ch1->iir[0].B2=0;
    ch1->iir[0].A1=0;
    ch1->iir[0].A2=0;

    ch1->iir[1].B0=1;
    ch1->iir[1].B1=0;
    ch1->iir[1].B2=0;
    ch1->iir[1].A1=0;
    ch1->iir[1].A2=0;

    ch1->iir[2].B0=0.000769;
    ch1->iir[2].B1=0.001538;
    ch1->iir[2].B2=0.000769;
    ch1->iir[2].A1=1.92081;
    ch1->iir[2].A2=-0.923885;


   
    ch2->InputMode=ENCODER_MODE;
    ch2->OutputMode=STEP_DIR_MODE;
    ch2->Vel=50000;
    ch2->Accel=200000;
    ch2->Jerk=3000000;
    ch2->P=0;
    ch2->I=0;
    ch2->D=0;
    ch2->FFAccel=0.001;
    ch2->FFVel=0.003;
    ch2->MaxI=200;
    ch2->MaxErr=24;
    ch2->MaxOutput=2000;
    ch2->DeadBandGain=0;
    ch2->DeadBandRange=20;
    ch2->InputChan0=2;
    ch2->InputChan1=0;
    ch2->OutputChan0=19;
    ch2->OutputChan1=0;
    ch2->MasterAxis=-1;
    ch2->LimitSwitchOptions=0x11f;
    ch2->LimitSwitchNegBit=139;
    ch2->LimitSwitchPosBit=0;
    ch2->SoftLimitPos=MaxZ;
    ch2->SoftLimitNeg=MinZ;
    ch2->InputGain0=.5;
    ch2->InputGain1=1;
    ch2->InputOffset0=0;
    ch2->InputOffset1=0;
    ch2->OutputGain=1;
    ch2->OutputOffset=0;
    ch2->SlaveGain=1;
    ch2->BacklashMode=BACKLASH_OFF;
    ch2->BacklashAmount=0;
    ch2->BacklashRate=0;
    ch2->invDistPerCycle=0;
    ch2->Lead=0;
    ch2->MaxFollowingError=12;
    ch2->StepperAmplitude=20;

    ch2->iir[0].B0=1;
    ch2->iir[0].B1=0;
    ch2->iir[0].B2=0;
    ch2->iir[0].A1=0;
    ch2->iir[0].A2=0;

    ch2->iir[1].B0=1;
    ch2->iir[1].B1=0;
    ch2->iir[1].B2=0;
    ch2->iir[1].A1=0;
    ch2->iir[1].A2=0;

    ch2->iir[2].B0=0.056495;
    ch2->iir[2].B1=0.112989;
    ch2->iir[2].B2=0.056495;
    ch2->iir[2].A1=1.22565;
    ch2->iir[2].A2=-0.451634;
   
/*
        if(ESTOP_BIT && ZLIM_BIT && XLIM_BIT && YLIM_BIT )
        {
            EnableAxisDest(Y,0);  //y
            EnableAxisDest(X,0);   //z
            EnableAxisDest(Z,0);   //x 
        //    SetBit(154);
          //  SetBit(152);
            //  SetBit(153);
        }
     
      */

   
     
    while(1){







        /*if(ReadBit(48)) { //soft limits disable and manual home sequence
            ClearStopImmediately();
            SetSoftLim(0);  //disable soft limits
            ClearBit(48);  
                while(~ReadBit(48)){
                    home();
                    WaitNextTimeSlice();
                }
            ClearBit(48);
            SetSoftLim(1);
        }
    */
    //home();
    //center();
    estop();
    if(CheckDoneXYZABC()==0) SetBit(155);
        else ClearBit(155);
    //spindleWDog();
    WaitNextTimeSlice();
    //SetZ();

    }
     
     
        }
     
void estop(void){


        if (!ReadBit(ESTOP_BIT) || !ReadBit(ZLIM_BIT) || !ReadBit(YLIM_BIT) || !ReadBit(XLIM_BIT))
        {
           
            if (ch2->Enable) DisableAxis(Z);  // axis still enabled?  - Disable it
            if (ch1->Enable) DisableAxis(Y);  // axis still enabled?  - Disable it
            if

(Message over 64 KB, truncated)